#include "app.h"



APP::APP() : communicationDevice(), AFE(), AFE_auxiliary(), temperatureSensor(), ota(){

    Global_resources();

    this->param.base_temperature = 0;
    this->param.current_temperature = 0;
    this->param.atm = 0;
    this->param.water_Density = 1000;
    this->param.gravity_acc = 9.80f;
    this->param.Initial_depth_of_insertion_into_water = 0.2;
    this->param.water_level = 0;
    memset(this->param.timestamp, 0, sizeof(this->param.timestamp));
    this->param.Location.longitude = 107.38775f;
    this->param.Location.latitude = 29.70312f;

}


APP::~APP() {

}


char* APP::BuildJsonStr(WaterLevelCalculationParameters_Typedef* data) {

    cJSON* root = cJSON_CreateObject();
    if (root == NULL) {
        return NULL;
    }

    cJSON* device = cJSON_CreateObject();
    if (device == NULL) {
        cJSON_Delete(root);
        return NULL;
    }
    cJSON_AddStringToObject(device, "id", "CH32_001");
    cJSON_AddStringToObject(device, "type", "upstream");
//    cJSON_AddStringToObject(device, "students", "232154101417lxy");
    cJSON_AddItemToObject(root, "device", device);

    cJSON_AddStringToObject(root, "timestamp", data->timestamp);


    cJSON* data_obj = cJSON_CreateObject();
    if (data_obj == NULL) {
        cJSON_Delete(root);
        return NULL;
    }

    cJSON_AddNumberToObject(data_obj, "atm", data->atm);
    cJSON_AddNumberToObject(data_obj, "tube_pressure", data->tube_pressure);
    cJSON_AddNumberToObject(data_obj, "current_temperature", data->current_temperature);
    cJSON_AddNumberToObject(data_obj, "base_temperature", data->base_temperature);
    cJSON_AddNumberToObject(data_obj, "Initial_depth_of_insertion_into_water", data->Initial_depth_of_insertion_into_water);
    cJSON_AddNumberToObject(data_obj, "water_level", data->water_level);
    cJSON_AddNumberToObject(data_obj, "longitude", data->Location.longitude);
    cJSON_AddNumberToObject(data_obj, "latitude", data->Location.latitude);

    cJSON_AddItemToObject(root, "data", data_obj);

    char* json_str = cJSON_PrintUnformatted(root);

    cJSON_Delete(root);

    return json_str;

}

void APP::task_perse_msg_from_communication(void* arg) {

    while (1) {
        if (xSemaphoreTake(CommunicationDevice_xBinarySemaphore, portMAX_DELAY) == pdTRUE) {
            CommunicationDevice::CommunicationDev_Data_Perse();
        }
       vTaskDelay(pdMS_TO_TICKS(50));
    }
}

void APP::task_atm(void* arg) {
    APP* app = static_cast<APP*> (arg);
    AFE_DIF_SIGNAL_Param_Typedef_t afe_atm_param = {
            .KNOWN_SIGNAL = 98900.0f,
            .CALIBRATION_SAMPLES = 10,
            .OFFSET = 0,
            .SENSITIVITY = 1,
            .MODE = SPEED_10HZ_DIF_SIGNAL_GET
    };
    CH32_LOGI("task_atm", "calibrate_dif_signal_atm_sensor");
    app->AFE_auxiliary.calibrate_dif_signal_sensor(afe_atm_param);
    CH32_LOGI("task_atm calibrate_dif_signal_sensor", "%d %.2f", app->AFE_auxiliary.afe_dif_signal_param.OFFSET, app->AFE_auxiliary.afe_dif_signal_param.SENSITIVITY);

    CH32_LOGI("task_atm", "calibrate_dif_signal_sensor OK, system will get atm to calibrate tube_pressure,  Please release sensor, you have 3s to do");
    vTaskDelay(pdMS_TO_TICKS(3000));

    float atm = 0;
    float sum = 0;
    while(1)
    {
        for (u8 i = 0; i < 5; i++) {
            sum += app->AFE_auxiliary.read_dif_signal_value();
            vTaskDelay(pdMS_TO_TICKS(20));
        }
        atm = sum / 5;
        sum = 0;

        CH32_LOGI("task_atm", "atm data %.2f pa", atm);

        xQueueSend(ATM_Result_msg_Queue, &atm, pdMS_TO_TICKS(100));

        vTaskDelay(pdMS_TO_TICKS(500));
    }
}


void APP::task_tube_pressure(void* arg) {
    APP* app = static_cast<APP*> (arg);

    float calibrated_atm = 0;

    AFE_DIF_SIGNAL_Param_Typedef_t afe_tube_pressure_param = {
            .KNOWN_SIGNAL = 98900.0f,
            .CALIBRATION_SAMPLES = 10,
            .OFFSET = 0,
            .SENSITIVITY = 1,
            .MODE = SPEED_10HZ_DIF_SIGNAL_GET
    };

    if (xQueueReceive(ATM_Result_msg_Queue, &calibrated_atm, portMAX_DELAY) == pdPASS) {
        afe_tube_pressure_param.KNOWN_SIGNAL = calibrated_atm;
        CH32_LOGI("task_tube_pressure", "Recive KNOWN_SIGNAL %.2f!!!!!!!!!!!", calibrated_atm);

    }

    app->AFE.calibrate_dif_signal_sensor(afe_tube_pressure_param);
    CH32_LOGI("task_tube_pressure calibrate_dif_signal_sensor", "%d %.2f", app->AFE.afe_dif_signal_param.OFFSET, app->AFE.afe_dif_signal_param.SENSITIVITY);

    float pressure = 0;
    float sum = 0;

    while(1)
    {
        for (u8 i = 0; i < 5; i++) {
            sum += app->AFE.read_dif_signal_value();
            vTaskDelay(pdMS_TO_TICKS(20));
        }
        pressure = sum / 5;
        sum = 0;

        CH32_LOGI("tube_pressure", "%.2f pa", pressure);

        xQueueSend(Tube_Pressure_Result_msg_Queue, &pressure, pdMS_TO_TICKS(100));

        vTaskDelay(pdMS_TO_TICKS(500));
    }
}


void APP::task_Temperature(void* arg) {
    APP* app = static_cast<APP*> (arg);
    float t = 0;

    float sum = 0;

    for (u8 i = 0; i < 5; i++) {
        sum += app->temperatureSensor.GetTemperature();
        vTaskDelay(pdMS_TO_TICKS(20));
    }

    app->param.base_temperature = sum / 5;
    sum = 0;

    CH32_LOGI("task_Temperature", "set base_temperature = %.2f", app->param.base_temperature);

    while(1)
    {
        for (u8 i = 0; i < 5; i++) {
            sum += app->temperatureSensor.GetTemperature();
            vTaskDelay(pdMS_TO_TICKS(20));
        }

        t = sum / 5;
        sum = 0;

        CH32_LOGI("task_Temperature", "temp: %.2f", t);
        xQueueSend(Tempareture_Result_msg_Queue, &t, pdMS_TO_TICKS(100));
        vTaskDelay(pdMS_TO_TICKS(500));
    }
}

void APP::task_WaterLevelCalculation(void* arg) {
    APP* app = static_cast<APP*>(arg);
    project_msg_typedef_t data = {0};
    WaterLevelCalculationParameters_Typedef WLCP = app->param;

    WLCP.water_level_0 = 0.05f;
    const float iteration_tolerance = 0.001f;
    const int max_iterations = 10;

    while (1) {
        if ((xQueueReceive(Tempareture_Result_msg_Queue, &data.Tempareture, pdMS_TO_TICKS(100)) == pdPASS) &&
            (xQueueReceive(ATM_Result_msg_Queue, &data.ATM, pdMS_TO_TICKS(100)) == pdPASS) &&
            (xQueueReceive(Tube_Pressure_Result_msg_Queue, &data.Tube_Pressure, pdMS_TO_TICKS(100)) == pdPASS)) {

            // 数据记录
            CH32_LOGI("ATM_Result_msg_Queue_Recived", "ATM: %.2f Pa", data.ATM);
            CH32_LOGI("ATM_Result_msg_Queue_Recived", "TubePressure: %.2f Pa", data.Tube_Pressure);
            CH32_LOGI("ATM_Result_msg_Queue_Recived", "Temperature: %.2f °C", data.Tempareture);

            // 温度校验
            if (data.Tempareture < -40.0f || data.Tempareture > 100.0f) {
                data.Tempareture = WLCP.base_temperature;
                CH32_LOGW("Temperature_Validation", "Invalid temperature, using base: %.2f °C", WLCP.base_temperature);
            }

            // 更新参数
            WLCP.atm = data.ATM;
            WLCP.current_temperature = data.Tempareture;
            WLCP.tube_pressure = data.Tube_Pressure;

            // 气压校验
            if (WLCP.tube_pressure < 0.9f * WLCP.atm || WLCP.tube_pressure > 1.5f * WLCP.atm) {
                WLCP.water_level = 0; // 无效数据
                CH32_LOGW("Pressure_Validation", "Invalid pressure range");
            } else {
                // 转换为开尔文温度
                const float T = WLCP.current_temperature + 273.15f;
                const float T0 = WLCP.base_temperature + 273.15f;

                // 迭代计算水位高度
                float h = WLCP.water_level; // 使用上次结果作为初始值
                int iterations = 0;
                float h_prev;

                do {
                    h_prev = h;
                    // 由理想气体状态方程与静力学平衡
                    float denominator = 1 - h/WLCP.water_level_0;
                    if (fabs(denominator) < 1e-6f) {
                        CH32_LOGW("Math_Warning", "Denominator接近零，h=%.4f", h);
                        break;
                    }
                    h = (WLCP.tube_pressure - WLCP.atm * (T/T0) / denominator)
                        / (WLCP.water_Density * WLCP.gravity_acc)
                        - WLCP.Initial_depth_of_insertion_into_water;
                    iterations++;
                } while (fabs(h - h_prev) > iteration_tolerance && iterations < max_iterations);

                // 记录迭代信息
                if (iterations >= max_iterations) {
                    CH32_LOGW("Iteration_Warning", "Not fully converged after %d iterations", iterations);
                }

                WLCP.water_level = h;
            }

            // 更新water_level_0
            WLCP.water_level_0 = WLCP.water_level;

            // 发送结果
            if (xQueueSend(WaterLevelCalculate_msg_Queue, &WLCP, pdMS_TO_TICKS(100)) != pdPASS) {
                CH32_LOGE("Queue_Error", "Failed to send result");
            }
        }
        vTaskDelay(pdMS_TO_TICKS(2000));
    }
}

void APP::task_MQTT_communication(void* arg) {
    APP* app = static_cast<APP*>(arg);

    WaterLevelCalculationParameters_Typedef WLCP = {0};

#if defined (USE_4G_MODE)
    static char cmd_buf[2048] = {0};
    // MQTT服务器参数配置
    ML307R_MQTT_Config_Typedef MMC = {
        .server_addr = "1033818uz84px.vicp.fun",
        .port = 47345,
        .pubtopic = "ML307R/01",
        .subtopic = "ML307R/#",
        .OTA_device_pub = "CH32_OTA/device_send",
        .OTA_device_sub = "CH32_OTA/device_recv",
        .client_id = "ML307R_Client",
        .user_name = "feiniao",
        .password = "3333"
    };


    // AT测试
    CH32_LOGI("MQTT_Config", "Testing AT command...");
    strcpy(cmd_buf, "AT\r\n");
    app->communicationDevice.send_msg(cmd_buf, strlen(cmd_buf));
    vTaskDelay(pdMS_TO_TICKS(1000));

    // 配置MQTT版本
    CH32_LOGI("MQTT_Config", "Setting MQTT version...");
    strcpy(cmd_buf, "AT+MQTTCFG=\"version\",4\r\n");
    app->communicationDevice.send_msg(cmd_buf, strlen(cmd_buf));
    vTaskDelay(pdMS_TO_TICKS(500));

    // 禁用SSL
    CH32_LOGI("MQTT_Config", "Disabling SSL...");
    strcpy(cmd_buf, "AT+MQTTCFG=\"ssl\",0,0\r\n");
    app->communicationDevice.send_msg(cmd_buf, strlen(cmd_buf));
    vTaskDelay(pdMS_TO_TICKS(500));

    // MQTT连接
    CH32_LOGI("MQTT_Config", "Connecting to MQTT server...");
    snprintf(cmd_buf, sizeof(cmd_buf),
            "AT+MQTTCONN=0,\"%s\",%d,\"%s\",\"%s\",\"%s\"\r\n",
            MMC.server_addr, MMC.port, MMC.client_id,
            MMC.user_name, MMC.password);
    app->communicationDevice.send_msg(cmd_buf, strlen(cmd_buf));
    vTaskDelay(pdMS_TO_TICKS(3000));

    // 检查连接状态
    CH32_LOGI("MQTT_Config", "Checking connection status...");
    strcpy(cmd_buf, "AT+MQTTSTATE=0\r\n");
    app->communicationDevice.send_msg(cmd_buf, strlen(cmd_buf));
    vTaskDelay(pdMS_TO_TICKS(1000));

    // 订阅主题
    CH32_LOGI("MQTT_Config", "Subscribing to topic...");
    snprintf(cmd_buf, sizeof(cmd_buf),
            "AT+MQTTSUB=0,\"%s\",1\r\n",
            MMC.subtopic);
    app->communicationDevice.send_msg(cmd_buf, strlen(cmd_buf));
    vTaskDelay(pdMS_TO_TICKS(1000));

    snprintf(cmd_buf, sizeof(cmd_buf),
            "AT+MQTTSUB=0,\"%s\",1\r\n",
            MMC.OTA_device_sub);
    app->communicationDevice.send_msg(cmd_buf, strlen(cmd_buf));
    vTaskDelay(pdMS_TO_TICKS(1000));

    snprintf(cmd_buf, sizeof(cmd_buf), "AT+CCLK?\r\n");
    app->communicationDevice.send_msg(cmd_buf, strlen(cmd_buf));

#endif


    while (1) {
        if ((xQueueReceive(WaterLevelCalculate_msg_Queue, &WLCP, pdMS_TO_TICKS(100)) == pdPASS)) {
#if defined (USE_4G_MODE)
            // 获取时间
            snprintf(cmd_buf, sizeof(cmd_buf), "AT+CCLK?\r\n");
            app->communicationDevice.send_msg(cmd_buf, strlen(cmd_buf));

            // 接收，处理时间
            static char timestamp_buffer[timestamp_buffer_size] = {0};
            if (xQueueReceive(Timestamp_msg_Queue, timestamp_buffer, pdMS_TO_TICKS(1000)) == pdPASS) {
                char* time_start = strchr(timestamp_buffer, '\"');
                if (time_start) {
                    time_start++;
                    char* time_end = strchr(time_start, '\"');
                    if (time_end) {
                        *time_end = '\0';

                        // 替换逗号为减号
                        char* comma_pos = strchr(time_start, ',');
                        if (comma_pos) *comma_pos = '-';

                        strncpy(WLCP.timestamp, time_start, sizeof(WLCP.timestamp)-1);
                        WLCP.timestamp[sizeof(WLCP.timestamp)-1] = '\0';
                    }
                }
            }

            char* msg = app->BuildJsonStr(&WLCP);
            CH32_LOGI("task_MQTT_communication", "msg size: %d", strlen(msg));
            if (msg != NULL) {
                // 发布消息（QoS=1）
                snprintf(cmd_buf, sizeof(cmd_buf),
                        "AT+MQTTPUB=0,\"%s\",1,0,0,%d,\"%s\"\r\n",
                        MMC.pubtopic, strlen(msg), msg);
                app->communicationDevice.send_msg(cmd_buf, strlen(cmd_buf));
                CH32_LOGI("MQTT_Publish", "Publishing to %s: %s", MMC.pubtopic, msg);
                free(msg);
            }
#endif
#if defined (USE_ESP32C3)
            char* msg = app->BuildJsonStr(&WLCP);
            app->communicationDevice.send_msg(msg, strlen(msg));
            CH32_LOGI("Upload", "Upload: %s", msg);
            free(msg);
#endif
        }
        vTaskDelay(pdMS_TO_TICKS(7000));
    }
}



void APP::task_OTA(void* arg) {
    APP* app = static_cast<APP*> (arg);

    while (1) {
        app->ota.run();
        vTaskDelay(pdMS_TO_TICKS(20));
    }
}


void APP::task_heartbeat(void* arg) {

    while (1) {
        CH32_LOGI("task_heartbeat", "Ch32 is running...");
        vTaskDelay(pdMS_TO_TICKS(5000));
    }
}

void APP::run() {
    if (xTaskCreate(task_MQTT_communication, "MQTT", 2048, this, 7, NULL) != pdPASS) {
        CH32_LOGE("APP::run", "Failed to create MQTT task!");
    }
    xTaskCreate(task_atm, "task_atm", 512, this, 6, NULL);
    xTaskCreate(task_tube_pressure, "task_tube_pressure", 512, this, 6, NULL);
    xTaskCreate(task_Temperature, "task_Temperature", 512, this, 6, NULL);
    xTaskCreate(task_WaterLevelCalculation, "task_WaterLevelCalculation", 1024, this, 6, NULL);
    xTaskCreate(task_perse_msg_from_communication, "task_perse_msg_from_communication", 1024, this, 5, NULL);

    xTaskCreate(task_OTA, "task_OTA", 1024, this, 5, NULL);
    xTaskCreate(task_heartbeat, "task_heartbeat", 512, this, 5, NULL);

    vTaskStartScheduler();
}



